function [ output ] = Controller( input )

%Environmental interference force
Tau1=0;
Tau2=0;
Tau3=0;

%Heavy vehicle
m11=646;
m22=837;
m33=155;
m23=0;
m32=0;
d11=303;
d22=425;
d33=74;
d23=0;
d32=0;

%parameter
k1=0.165;k2=10;k3=3.5;k4=1;k5=1;the1=0.1; the2=0.1;

x_e=input(1);
S1=input(2);
S2=input(3);
p_sigma1=input(4);
p_sigma3=input(5);
u_e=input(6);
r_e=input(7);
gemma_e=input(8);
psi_e=input(9);
K=input(10);
      

Tau_u=-p_sigma1-k3*m11*u_e-x_e+u_e-sat(S1,the1);
%Tau_u=-p_sigma1-k3*m11*u_e-x_e+u_e;
Tau_r=-p_sigma3-k5*m33*r_e-(m22/m33)*gemma_e*K*cos(psi_e)+r_e-sat(S2,the2);
%Tau_r=-p_sigma3-k5*m33*r_e-(m22/m33)*gemma_e*K*cos(psi_e)+r_e;



output=[Tau_u Tau_r];

function cof=sat(S,the)
  if abs(S)<=the
      cof=S/the;
  else
      cof=sign(S);
  end
